/*=============================================================================

  MOTION CONTROL HOMING IMPLEMENTATION

  FILE: MOT_HOME.cpp
  
  DESCRIPTION

    Motion custom homing implemenation
    
  EXTERNALIZED FUNCTIONS


   Copyright (c) DaeSung Engineering Inc. All rights reserved.

=============================================================================*/
/*=============================================================================

  EDIT HISTORY

  ---------  -------  -------------------------------------------------------
    when       who       what, where, why
  ---------  -------  -------------------------------------------------------
   07/15/04   nso4th    Initial version

=============================================================================*/

/*=============================================================================
  HEADER INCLUSION
=============================================================================*/
#include "MOTFTbl.h"
/*=============================================================================
  CONSTANT DEFINITION
=============================================================================*/
#define MID_SPEED_SCALE			(5.0)
#define MID_ACCEL_SCALE			(5.0)

#define LOW_SPEED_SCALE			(10.0)
#define LOW_ACCEL_SCALE			(10.0)


/*=============================================================================
  LOCALIZED FUNCTION DEFINITION
=============================================================================*/
static void HOME_WAIT(dword wait_time);

/*=============================================================================
  GENERAL HOMING ROUTINE
=============================================================================*/
dword MOTFTbl::HOMEPROC_GEN(MOTHOMEItem_s *pItem)
{
	dword			dwRet = MOTHOME_NEXT_R;
	boolean			bSig = FALSE;
	motdev_axis_s		*pAxis;
	mcxt			pDev;
	double64			vel, acc;
	
	pAxis = DEVICE_AXIS(pItem->axis);
	pDev = DEVICE_ACTUAL(pAxis);


	switch(pItem->dwStep)
	{
	case MOTHOME_STEP_START:
	{
		pItem->dwStep = MOTHOME_STEP_0;
		SWLimitEnable(pItem->axis, FALSE);
		//PosSet(pItem->axis, 0.0);
		break;
	}
	// CHECK N-LIMIT
	case MOTHOME_STEP_0:
	{
		SigGet(pItem->axis, MOT_SIG_NLIMIT, &bSig);

		if(bSig)
		{
			// Find the negative origin
			Stop(pItem->axis);
			pItem->dwStep = MOTHOME_STEP_2;
		}
		else
		{
			// Find the negative origin
			pItem->dwStep = MOTHOME_STEP_1;
		}
		break;
	}
	// ANY POSITION ===> N-LIMIT
	case MOTHOME_STEP_1:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel =  (-1.0)*(pItem->dVel);
			acc = pItem->dAccel;
			
			MoveToSignal(pItem->axis, MOT_SIG_NLIMIT_PEDGE,	vel, acc);
			pItem->dwStep = MOTHOME_STEP_2;
		}
		break;
	}
	// N-LIMIT ===> NEGATIVE ORIGIN
	case MOTHOME_STEP_2:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel = pItem->dVel/MID_SPEED_SCALE;
			acc = pItem->dAccel/MID_ACCEL_SCALE;
			MoveToSignal(pItem->axis, MOT_SIG_ORIGIN_NEDGE, vel, acc);
			pItem->dwStep = MOTHOME_STEP_6;
		}
		break;
	}
	// NEGATIVE ORIGIN ===> REF. INDEX
	case MOTHOME_STEP_3:  // Find the origin sensor to -direction
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel = pItem->dVel/LOW_SPEED_SCALE;
			acc = pItem->dAccel/LOW_ACCEL_SCALE;

			MoveToSignal(pItem->axis, MOT_SIG_ENC_INDEX_PEDGE, vel, acc);
			pItem->dwStep = MOTHOME_STEP_4;
		}
		break;
	}
	// WAIT REF. INDEX
	case MOTHOME_STEP_4:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			double64		offset;
			
			offset = GetHomeZeroOffset(pItem->axis);
			if(offset == (0.0))
			{
				pItem->dwStep = MOTHOME_STEP_6;
			}
			else
			{
				pItem->dwStep = MOTHOME_STEP_5;
			}
		}
		break;
	}
	// MOVE TO THE ZERO OFFSET
	case MOTHOME_STEP_5:
	{
		double64		dOffset, dCur;
			
		dOffset = GetHomeZeroOffset(pItem->axis);
		PosGet(pItem->axis, &dCur);
		Move(pItem->axis, (dCur+dOffset), pItem->dVel, pItem->dAccel);
		pItem->dwStep = MOTHOME_STEP_6;
		break;
	}
	// WAIT FINAL POSITION (ZERO OFFSET)
	case MOTHOME_STEP_6:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			pItem->dwStep = MOTHOME_STEP_DONE;
		}
		break;
	}
	// ANY POSITION ===> P-LIMIT
	case MOTHOME_STEP_7:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel =  (1.0)*(pItem->dVel);
			acc = pItem->dAccel;
			
			MoveToSignal(pItem->axis, MOT_SIG_PLIMIT_PEDGE,	vel, acc);
			pItem->dwStep = MOTHOME_STEP_8;
		}
		break;
	}
	// N-LIMIT ===> NEGATIVE ORIGIN
	case MOTHOME_STEP_8:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel = (-1.0)*(pItem->dVel/MID_SPEED_SCALE);
			acc = pItem->dAccel/MID_ACCEL_SCALE;
			MoveToSignal(pItem->axis, MOT_SIG_ORIGIN_NEDGE, vel, acc);
			pItem->dwStep = MOTHOME_STEP_6;
		}
		break;
	}
	case MOTHOME_STEP_DONE:
	{
		// Wait 500msec.
		/*		----------------------2013. 10. 18. jyko
		if((pItem->axis == MOT_AXIS_X) || (pItem->axis == MOT_AXIS_Y))
		{
			HOME_WAIT(1500);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(500);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(200);
		}
		else
		*/
		{
			HOME_WAIT(500);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(200);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(200);
		}
		
		SWLimitEnable(pItem->axis, TRUE);
		dwRet = MOTHOME_DONE_R;
		break;
	}
	
	default:
		return	MOTHOME_ERROR_R;
	}
	
	SigGet(pItem->axis, MOT_SIG_ALARM, &bSig);

	if(bSig == TRUE)
	{
		DBGPRINT("[%d] ALARM TRUE\n", pItem->dwStep);
		dwRet = MOTHOME_ERROR_R;
	}
	
	
	return	dwRet;
}

dword MOTFTbl::HOMEPROC_PLMT(MOTHOMEItem_s *pItem)
{
	dword			dwRet = MOTHOME_NEXT_R;
	boolean			bSig = FALSE;
	motdev_axis_s		*pAxis;
	mcxt			pDev;
	double64			vel, acc;
	
	pAxis = DEVICE_AXIS(pItem->axis);
	pDev = DEVICE_ACTUAL(pAxis);


	switch(pItem->dwStep)
	{
	case MOTHOME_STEP_START:
	{
		pItem->dwStep = MOTHOME_STEP_0;
		SWLimitEnable(pItem->axis, FALSE);
		//PosSet(pItem->axis, 0.0);
		break;
	}
	// CHECK N-LIMIT
	case MOTHOME_STEP_0:
	{
		SigGet(pItem->axis, MOT_SIG_NLIMIT, &bSig);

		if(bSig)
		{
			// Find the negative origin
			Stop(pItem->axis);
			pItem->dwStep = MOTHOME_STEP_2;
		}
		else
		{
			// Find the negative origin
			pItem->dwStep = MOTHOME_STEP_1;
		}
		break;
	}
	// ANY POSITION ===> N-LIMIT
	case MOTHOME_STEP_1:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel =  (-1.0)*(pItem->dVel);
			acc = pItem->dAccel;
			
			MoveToSignal(pItem->axis, MOT_SIG_NLIMIT_PEDGE,	vel, acc);
			pItem->dwStep = MOTHOME_STEP_2;
		}
		break;
	}
	// N-LIMIT ===> NEGATIVE ORIGIN
	case MOTHOME_STEP_2:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel = pItem->dVel/MID_SPEED_SCALE;
			acc = pItem->dAccel/MID_ACCEL_SCALE;
			MoveToSignal(pItem->axis, MOT_SIG_ORIGIN_NEDGE, vel, acc);
			pItem->dwStep = MOTHOME_STEP_3;
		}
		break;
	}
	// NEGATIVE ORIGIN ===> REF. INDEX
	case MOTHOME_STEP_3:  // Find the origin sensor to -direction
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			double64			dCur;
			vel = pItem->dVel;
			acc = pItem->dAccel;

			PosGet(pItem->axis, &dCur);
			Move(pItem->axis, (dCur+150.0), vel, acc);

			pItem->dwStep = MOTHOME_STEP_4;
		}
		break;
	}
	// WAIT REF. INDEX
	case MOTHOME_STEP_4:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			pItem->dwStep = MOTHOME_STEP_5;
		}
		break;
	}
	
	// NEGATIVE ORIGIN ===> REF. INDEX
	case MOTHOME_STEP_5:  // Find the origin sensor to -direction
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			vel = pItem->dVel/LOW_SPEED_SCALE;
			acc = pItem->dAccel/LOW_ACCEL_SCALE;

			MoveToSignal(pItem->axis, MOT_SIG_ENC_INDEX_PEDGE, vel, acc);
			pItem->dwStep = MOTHOME_STEP_6;
		}
		break;
	}
	// WAIT REF. INDEX
	case MOTHOME_STEP_6:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			double64		offset;
			
			offset = GetHomeZeroOffset(pItem->axis);
			if(offset == (0.0))
			{
				pItem->dwStep = MOTHOME_STEP_8;
			}
			else
			{
				pItem->dwStep = MOTHOME_STEP_7;
			}
		}
		break;
	}
	// MOVE TO THE ZERO OFFSET
	case MOTHOME_STEP_7:
	{
		double64		dOffset, dCur;
			
		dOffset = GetHomeZeroOffset(pItem->axis);
		PosGet(pItem->axis, &dCur);
		Move(pItem->axis, (dCur+dOffset), pItem->dVel, pItem->dAccel);
		pItem->dwStep = MOTHOME_STEP_8;
		break;
	}
	// WAIT FINAL POSITION (ZERO OFFSET)
	case MOTHOME_STEP_8:
	{
		MoveDone(pItem->axis, &bSig);
		if(bSig)
		{
			pItem->dwStep = MOTHOME_STEP_DONE;
		}
		break;
	}
	case MOTHOME_STEP_DONE:
	{
		// Wait 500msec.
		/*		----------------------2013. 10. 18. jyko
		if((pItem->axis == MOT_AXIS_X) || (pItem->axis == MOT_AXIS_Y))
		{
			HOME_WAIT(1500);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(500);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(200);
		}
		else
		*/
		{
			HOME_WAIT(500);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(200);
			PosSet(pItem->axis, 0.0);
			HOME_WAIT(200);
		}
		
		SWLimitEnable(pItem->axis, TRUE);
		dwRet = MOTHOME_DONE_R;
		break;
	}
	default:
		return	MOTHOME_ERROR_R;
	}
	
	return	dwRet;
}



void HOME_WAIT(dword wait_time)
{
	int32		j, count;

	count = (int32)wait_time/10;
	if(count < 0)
	{
		Task_Delay(10);
		Task_Switch();
		return;
	}
	for(j=0; j<count; j++)
	{
		Task_Delay(10);
		Task_Switch();
	}
}

/*=============================================================================
  UMAC HOMING ROUTINE
=============================================================================*/
#define MOTHOME_STEP_WAITTIME		(MOTHOME_STEP_0)

dword MOTFTbl::HOMEPROC_CQIHAL(MOTHOMEItem_s *pItem)
{
	dword			dwRet = MOTHOME_NEXT_R;
	boolean			bSig = FALSE;
	motdev_axis_s	*pAxis;
	static mcxt		st = NULL;
	double64			elapse;

	pAxis = DEVICE_AXIS(pItem->axis);
	
	switch(pItem->dwStep)
	{
	case MOTHOME_STEP_START:
	{
		pItem->dwStep = MOTHOME_STEP_WAITTIME;
			
		CQIHAL_Home(pItem->axis);

		if(st != NULL)
		{
			Time_STFree(st);
			st = NULL;
		}
		st = Time_STAlloc();
		Time_STTick(st);

		DBGPRINT("[AXIS%d] HOME START\n",pItem->axis);
		break;
	}
	case MOTHOME_STEP_WAITTIME:  // 'ENAPLC..' COMMAND wait
	{
		Time_STTock(st, &elapse);
		
		if(elapse >= 1.0)
		{
			pItem->dwStep = MOTHOME_STEP_1;
			Time_STFree(st);
			st = NULL;
		}
		else
		{
		}
		break;
	}
	case MOTHOME_STEP_1:  // Detect N-Limit
	{
		if(MoveDone(pItem->axis, &bSig) == FALSE)
		{
			DBGPRINT("[%dM]", pItem->axis);
			return	MOTHOME_ERROR_R;
		}
		if(bSig == TRUE)
		{
			if(CQIHAL_IsHomeDone(pItem->axis, &bSig) == FALSE)
			{
				DBGPRINT("[%dH]", pItem->axis);
				return	MOTHOME_ERROR_R;
			}
			if(bSig)
			{
				//LOGWARN("[UMAC%d] HOME_DONE !", pItem->axis);
				pItem->dwStep = MOTHOME_STEP_DONE;
			}
		}
		break;
	}
	case MOTHOME_STEP_DONE:
	{
		//LOGWARN("[UMAC%d] HOME_FINISHED !", pItem->axis);
		dwRet = MOTHOME_DONE_R;
		break;
	}
	default:
		return	MOTHOME_ERROR_R;
	}

	return	dwRet;
}

boolean MOTFTbl::CQIHAL_Home(int32 axID)
{
	MOT_s			*pMot = MOTCXT(m_pCxt);
	motdev_axis_s	*axCxt = DEVICE_AXIS(axID);
	mcxt			pDev = DEVICE_ACTUAL(axCxt);
	int32			axDev;
	
	if(axCxt == NULL)
		return	FALSE;

	axDev = axCxt->devModID;

	//return	(boolean)MotDev_Execute(pDev, UMAC_HOME_C, axDev, 0);
	return	(boolean)MotDev_Execute(pDev, MOTHOME_NONE_R, axDev, 0);
}

boolean MOTFTbl::CQIHAL_HomeStop(int32 axID)
{
	MOT_s			*pMot = MOTCXT(m_pCxt);
	motdev_axis_s	*axCxt = DEVICE_AXIS(axID);
	mcxt			pDev = DEVICE_ACTUAL(axCxt);
	int32			axDev;

	if(axCxt == NULL)
		return	FALSE;

	axDev = axCxt->devModID;

	return (boolean)MotDev_Execute(pDev, MOTHOME_NONE_R, axDev, 0);
}

boolean MOTFTbl::CQIHAL_IsHomeDone(int32 axID, boolean *pDone)
{
	MOT_s			*pMot = MOTCXT(m_pCxt);
	motdev_axis_s	*axCxt = DEVICE_AXIS(axID);
	mcxt			pDev = DEVICE_ACTUAL(axCxt);
	int32			axDev;
	boolean			bDone;

	if(axCxt == NULL)
		return	FALSE;

	axDev = axCxt->devModID;

	if(MotDev_Execute(pDev, MOTHOME_DONE_R, axDev, (dword)(&bDone)))
	{
		*pDone = bDone;
		return	TRUE;
	}

	return	FALSE;
}

/*--- END OF MOTFuncTbl.CPP ---*/
